
//
// Created by jiang on 2021/12/20.
//

#include "sintable.h"
#include "foc.h"

static const int one_by_sqrt3 = 577;
static const int two_by_sqrt3 = 1155;
static const int sqrt3 = 1732;

static int sin, cos;

/* Equal amplitude Clarke transformation
 * |a|         |(1)         (-1/2)      (-1/2)      |   |a|
 * |p| = 2/3 * |(0)         (sqrt(3)/2) -(sqrt(3)/2)| * |b|
 * |0|         |(sqrt(2)/2) (sqrt(2)/2) (sqrt(2)/2) |   |c|
 */
void FOC_Clarke(const abc_t *abc, ap_t *ap) {
#if defined(USE_CURR_AB)
    /* Ialpha = Ia*/
    ap->alpha = abc->a;
    /*Ibeta = (2*Ib+Ia)/sqrt(3)*/
    ap->beta = (2 * abc->b + abc->a) * 1000 / sqrt3;

#elif defined(USE_CURR_BC)
    /* Ialpha = -Ib - Ic*/
    ap->alpha = -abc->b - abc->c;
    /*Ibeta = (Ib - Ic)/sqrt(3)*/
    ap->beta = (abc->b - abc->c) * 1000 / sqrt3;

#elif defined(USE_CURR_AC)
    /* Ialpha = Ia*/
    ap->alpha = abc->a;
    /*Ibeta = (-2 * Ic - Ia)/sqrt(3)*/
    ap->beta = 1000 * (-2 * abc->c - abc->a) / sqrt3;
#endif
}


void FOC_Park(const ap_t *ap, const uint16_t theta, dq_t *dq) {
    sin = arm_sin_f32(theta);
    cos = arm_cos_f32(theta);
    dq->d = (ap->alpha * cos + ap->beta * sin) >> 14; // divisor by 16384
    dq->q = (ap->beta * cos - ap->alpha * sin) >> 14;
}


void FOC_InvPark(const dq_t *dq, ap_t *ap) {
    ap->alpha = (dq->d * cos - dq->q * sin) >> 14;
    ap->beta = (dq->d * sin + dq->q * cos) >> 14;
}

void FOC_InvParkWithAngle(const dq_t *dq, uint16_t theta, ap_t *ap) {
    sin = arm_sin_f32(theta);
    cos = arm_cos_f32(theta);
    ap->alpha = (dq->d * cos - dq->q * sin) >> 14;
    ap->beta = (dq->d * sin + dq->q * cos) >> 14;
}

uint8_t FOC_SVPWM(const ap_t *ap, abc_t *t) {
    static int u1, u2, u3;
    static int t_first, t_second, total;
    static uint8_t A, B, C, sector;

    u1 = ap->beta;
    u2 = (sqrt3 * ap->alpha -  1000 * ap->beta) / 2000;
    u3 = (-sqrt3 * ap->alpha - 1000 * ap->beta) / 2000;

    A = (u1 > 0) ? (1) : (0);
    B = (u2 > 0) ? (1) : (0);
    C = (u3 > 0) ? (1) : (0);

    switch ((C << 2) + (B << 1) + A) {
        case 3: {
            sector = 1;
            t_first = two_by_sqrt3 * u2 / 1000;//t4
            t_second = two_by_sqrt3 * u1 / 1000;//t6

            if(t_first + t_second > 1000){
                total = t_first + t_second;
                t_first = t_first * 1000 / total;
                t_second = t_second * 1000 / total;
            }

            t->a = (1000 - t_first - t_second) >> 1;
            t->b = t->a + t_first;
            t->c = t->b + t_second;
        }
            break;

        case 1: {
            sector = 2;
            t_second = -two_by_sqrt3 * u3 / 1000;//t6
            t_first = -two_by_sqrt3 * u2 / 1000;//t2

            if(t_first + t_second > 1000){
                total = t_first + t_second;
                t_first = t_first * 1000 / total;
                t_second = t_second * 1000 / total;
            }

            t->b = (1000 - t_first - t_second) >> 1;
            t->a = t->b + t_first;
            t->c = t->a + t_second;
        }
            break;
        case 5: {
            sector = 3;
            t_first = two_by_sqrt3 * u1 / 1000;//t2
            t_second = two_by_sqrt3 * u3 / 1000;//t3

            if(t_first + t_second > 1000){
                total = t_first + t_second;
                t_first = t_first * 1000 / total;
                t_second = t_second * 1000 / total;
            }

            t->b = (1000 - t_first - t_second) >> 1;
            t->c = t->b + t_first;
            t->a = t->c + t_second;
        }
            break;
        case 4: {
            sector = 4;
            t_second = -two_by_sqrt3 * u2 / 1000;//t3
            t_first = -two_by_sqrt3 * u1 / 1000;//t1

            if(t_first + t_second > 1000){
                total = t_first + t_second;
                t_first = t_first * 1000 / total;
                t_second = t_second * 1000 / total;
            }

            t->c = (1000 - t_first - t_second) >> 1;
            t->b = t->c + t_first;
            t->a = t->b + t_second;
        }
            break;
        case 6: {
            sector = 5;
            t_first = two_by_sqrt3 * u3 / 1000;//t1
            t_second = two_by_sqrt3 * u2 / 1000;//t5

            if(t_first + t_second > 1000){
                total = t_first + t_second;
                t_first = t_first * 1000 / total;
                t_second = t_second * 1000 / total;
            }

            t->c = (1000 - t_first - t_second) >> 1;
            t->a = t->c + t_first;
            t->b = t->a + t_second;
        }
            break;
        case 2: {
            sector = 6;
            t_second = -two_by_sqrt3 * u1 / 1000;//t5
            t_first = -two_by_sqrt3 * u3 / 1000;//t4

            if(t_first + t_second > 1000){
                total = t_first + t_second;
                t_first = t_first * 1000 / total;
                t_second = t_second * 1000 / total;
            }

            t->a = (1000 - t_first - t_second) >> 1;
            t->c = t->a + t_first;
            t->b = t->c + t_second;
        }
            break;
        default:
            sector = 0;
    }

    return sector;
}

